#include "LinAlg.h"

// column major - vectors are column vectors
Matrix4::Matrix4(){
    for(int i = 0; i < 16; i++)
      M[i] = 0.0;
}

void Matrix4::scaleNonUni(float x, float y, float z){
  float A[16] = {x, 0.0, 0.0, 0.0,
                 0.0, y, 0.0, 0.0,
                 0.0, 0.0, z, 0.0,
                 0.0, 0.0, 0.0, 1};
	for(int i = 0; i<16; i++)
		M[i] = A[i];  
}

void Matrix4::scaleUni(float s){
  scaleNonUni(s,s,s);
}
  
float* Matrix4::getPointer(){
  float* array = (float*) malloc(16*sizeof(float));
	for(int i = 0; i<16; i++)
		array[i] = M[i];
	return &array[0];
}

void Matrix4::set(float *A){
	for(int i = 0; i < 16; i++)
	  M[i] = A[i];
}

void Matrix4::set(int i, float v){
	M[i] = v;
}

float Matrix4::get(int i){
	return M[i];
}

float* Matrix4::get(){
  float* res = (float*) malloc(16*sizeof(float));
	for(int i = 0; i < 16; i++)
	  res[i] = M[i];
	return res;
}

void Matrix4::rotationX(float rot){
	double th = rot * M_PI / 180.f;
  float A[16] = {1.0, 0.0, 0.0, 0.0,
	               0.0, cos(th), -sin(th), 0.0,
            	   0.0, sin(th), cos(th), 0.0,
            	   0.0, 0.0, 0.0, 1.0};
for(int i = 0; i < 16; i++)
  M[i] = A[i];
}

void Matrix4::rotationY(float rot){
	double th = rot * M_PI / 180.f;
  float A[16] = {cos(th), 0.0, -sin(th), 0.0,
            	   0.0, 1.0, 0.0, 0.0,
            	   sin(th), 0.0, cos(th), 0.0,
            	   0.0, 0.0, 0.0, 1.0};
  for(int i = 0; i < 16; i++)
    M[i] = A[i];  
}

void Matrix4::rotationZ(float rot){
	double th = rot * M_PI / 180.f;
  float A[16] = {cos(th), sin(th), 0.0, 0.0,
          	   -sin(th), cos(th), 0.0, 0.0,
          	   0.0, 0.0, 1.0, 0.0,
          	   0.0, 0.0, 0.0, 1.0};
for(int i = 0; i < 16; i++)
  M[i] = A[i];
}

void Matrix4::rotationArbitrary(Vector3 a, float rot){
float A[16] = {1 + (1 - cos(rot)) * (a.x * a.x - 1),
	   a.z * sin(rot) + (1 - cos(rot)) * a.y * a.x,
	   -a.y * sin(rot) + (1 - cos(rot)) * a.z * a.x, 0,// first column
	   -a.z * sin(rot) + (1 - cos(rot)) * a.x * a.y,
	   1 + (1 - cos(rot)) * (a.y * a.y - 1),
	   a.x * sin(rot) + (1 - cos(rot)) * a.z * a.y, 0,// second column
	   a.y * sin(rot) + (1 - cos(rot)) * a.x * a.z,
	   -a.x * sin(rot) + (1 - cos(rot)) * a.y * a.z,
	   1 + (1 - cos(rot)) * (a.z * a.z - 1), 0, // third column
	   0,0,0,1}; 
for(int i = 0; i < 16; i++)
  M[i] = A[i];
}

void Matrix4::multiply(Matrix4 a){
  float mat[16];
  float tmp = 0;
  for(int y = 0; y < 4; y++){
    for(int x = 0; x < 4; x++){
      for(int i = 0; i < 4; i++){
        tmp += get(i*4+y) * a.get(i+x*4);
      }
      mat[y+4*x] = tmp;
      tmp = 0;
    }
  }
  for(int i = 0; i < 16; i++)
    M[i] = mat[i];
}

void Matrix4::multiply(float* a){
  float mat[16];
  float tmp = 0;
  for(int y = 0; y < 4; y++){
    for(int x = 0; x < 4; x++){
      for(int i = 0; i < 4; i++){
        tmp += M[i*4+y] * a[i+x*4];
      }
      mat[y+4*x] = tmp;
      tmp = 0;
    }
  }
  for(int i = 0; i < 16; i++)
    M[i] = mat[i];
}

Vector3 Matrix4::vector3Mul(Vector3 b){
  Vector4 *c = new Vector4;
  c->set(b.x, b.y, b.z, 0);
  *c = vector4Mul(*c);
  b.set(c->x, c->y, c->z);
  return b;
}

void Matrix4::identity(){
  float A[16] = {1.0, 0.0, 0.0, 0.0,
	     0.0, 1.0, 0.0, 0.0,
	     0.0, 0.0, 1.0, 0.0,
	     0.0, 0.0, 0.0, 1.0};
  for(int i = 0; i < 16; i++)
    M[i] = A[i];
}

Vector4 Matrix4::vector4Mul(Vector4 b){
  Vector4 *c = new Vector4;
  float list[4];
  float tmp = 0;
  for(int i = 0; i < 4; i++){
    for(int j = 0; j < 4; j++){
      tmp += M[j*4+i] * b[j];
    }
    list[i] = tmp;
    tmp = 0;
  }
  c->set(list[0], list[1], list[2], list[3]);
  return *c;
}

void Matrix4::translate(float a, float b, float c){
  identity();
  M[12] = a;
  M[13] = b;
  M[14] = c;
}

#include <stdio.h>

void Matrix4::printMatrix(){
  for(int i = 0; i < 4; i++){
    for(int j = 0; j < 4; j++){
      printf("\t%.2f,", get(j*4+i));
    }
    std::cout << std::endl;
  }
  std::cout << std::endl;
}
